package moro;

import java.awt.Color;
import java.awt.Dimension;
import java.awt.Font;
import java.awt.Graphics;
import java.awt.SystemColor;
import javax.swing.JPanel;

/**
 * Title:       The MObile RObot Simulation Environment
 * Description:
 * Copyright:   Copyright (c) 2001
 * Company:     Universit� di Bergamo
 * @author Brugali, Bleker, van Zutphen, Stellingwerf, Grobben
 * @version 1.0
 */

@SuppressWarnings("serial")
public class OccupancyMap extends JPanel {
    // JB: The class OccupanceMap is responsible for containing the data that has been discovered by
    // the robot. It is also reponsible for drawing this data. Not a great example of cohesion.

    int cellDim = 10;
    int width   = 510;
    int height  = 460;
    boolean mapIsExplored = false;
    
    char grid[][] = new char[width / cellDim][height / cellDim];
    char gridOverlay[][] = new char[width / cellDim][height / cellDim];

    private final static char UNKNOWN        = 'u';
    private final static char EMPTY          = 'e';
    private final static char OBSTACLE       = 'l';
    private final static char OPAQUEOBSTACLE = 's';
    private final static char TOUCHED        = 't';

    public OccupancyMap() {
        this.setBackground(SystemColor.window);
        this.setPreferredSize(new Dimension(510, 460));//Prefered size based on amount of grids

        for (int i = 0; i < width / cellDim; i++) {
            for (int j = 0; j < height / cellDim; j++) {
                grid[i][j]        = UNKNOWN;
                gridOverlay[i][j] = UNKNOWN;
            }
        }
        this.repaint();
    }

    @Override
    public void paintComponent(Graphics g) {
        super.paintComponent(g);
        // --------------------------- draw map --------------------------------------
        for (int i = 0; i < width / cellDim; i++) {
            for (int j = 0; j < height / cellDim; j++) {
                if (grid[i][j] == UNKNOWN) {
                    g.setColor(Color.yellow);
                    g.fillRect(i * cellDim, j * cellDim, cellDim, cellDim);
                } else if (grid[i][j] == OBSTACLE) {
                    g.setColor(Color.blue);
                    g.fillRect(i * cellDim, j * cellDim, cellDim, cellDim);
                } else if (grid[i][j] == OPAQUEOBSTACLE) {
                    g.setColor(Color.darkGray);
                    g.fillRect(i * cellDim, j * cellDim, cellDim, cellDim);
                }
    
                // --- teken reeds afgelegde weg dmv overlay grid ----
                if(gridOverlay[i][j] == TOUCHED) {
                    g.setColor(Color.pink);
                    g.fillRect(i * cellDim, j * cellDim, cellDim, cellDim);
                }
                

            }
        }
        // --------------------------- draw grid --------------------------------------
        g.setColor(Color.lightGray);
        for (int i = 0; i <= width / cellDim; i++) {
            g.drawLine(i * cellDim, 0, i * cellDim, height);
        }
        for (int j = 0; j <= height / cellDim; j++) {
            g.drawLine(0, j * cellDim, width, j * cellDim);
        }
        // map compleet explored in controlwindow tonen.
        if(mapIsExplored) {
            g.setColor(Color.red);
            Font font1 = new Font("Helvetica", Font.BOLD,  28);
            g.setFont(font1);
            g.drawString("3-CPO: Mission Accomplished", 30, 200);
        }
    }

    /**
     *  Deze methode tekent de resultaten van een scan op het scherm
     *  Het type sensor wordt meegegeven, op grond hiervan worden gedeteceerde obstacles
     *  in verschillende kleuren weergegeven. Blauw voor de Laser, Roze voor de Sonar 
     * 
     * @param position
     * @param measures
     * @param sensor Laser of Sonar
     * @param sensorRange detection range of Sensor 
     */
    public void drawSensorScan(double position[], double measures[], Device sensor, int sensorRange) {
        // correctie groot 20cm: sensorpunt robot staat 20 cm voor referentiepunt (=position) robot
        // hier wordt dus vastgesteld waar het sensorpunt zich exact bevindt
        double rx = Math.round(position[0] + 20.0 * Math.cos(Math.toRadians(position[2])));
        double ry = Math.round(position[1] + 20.0 * Math.sin(Math.toRadians(position[2])));
        int dir = (int) Math.round(position[2]);

        for (int i = 0; i < 360; i++) {
            int d = i - dir;
            while (d < 0) {
                d += 360;
            }
            while (d >= 360) {
                d -= 360;
            }
            // coordinaten sensorpunt + afstand meting geeft de positie van het nieuwe punt
            // dit is einde sensorbeam (niets gedetecteerd) of rand obstacles
            double fx = Math.round(rx + measures[d] * Math.cos(Math.toRadians(i)));
            double fy = Math.round(ry + measures[d] * Math.sin(Math.toRadians(i)));

            // obstacles gevonden
            if (measures[d] < sensorRange) {
                drawSensorBeam(rx, ry, fx, fy, true, sensor);
            } else if (sensor instanceof Sonar && measures[d] == (double)sensorRange) {
                // nothing
            } // niets gedetecteerd
            else {
                drawSensorBeam(rx, ry, fx, fy, false, sensor);
            }
        }

    }

    /**
     *  Tekent map op grond van gebruikte soort Sensor
     *  Voor door de Laser gedetecteerde obstacles worden andere kleuren gebruikt dan voor de Sonar
     * @param rx
     * @param ry
     * @param x
     * @param y
     * @param obstacle
     * @param sensor 
     */
    private void drawSensorBeam(double rx, double ry, double x, double y, boolean obstacle, Device sensor) {
        // ceil = the smallest (closest to negative infinity) floating-point value
        //        that is not less than the argument and is equal to a mathematical integer.
        int rxi = (int) Math.ceil(rx / cellDim);
        int ryj = (int) Math.ceil(ry / cellDim);
        int xi = (int) Math.ceil(x / cellDim);
        int yj = (int) Math.ceil(y / cellDim);

        // niet buiten bereik occupancy grid treden
        if (xi < 0 || yj < 0 || xi >= width / cellDim || yj >= height / cellDim) {
            return;
        }

        // kleur Map afhankelijk van gebruikte Sensor
        if (obstacle) {
            if (sensor instanceof Laser && grid[xi][yj] != OPAQUEOBSTACLE) {
                grid[xi][yj] = OBSTACLE;
            } else if (sensor instanceof Sonar && grid[xi][yj] != OBSTACLE) {
                grid[xi][yj] = OPAQUEOBSTACLE;
            }
        }

        int xmin = Math.min(rxi, xi);
        int xmax = Math.max(rxi, xi);
        int ymin = Math.min(ryj, yj);
        int ymax = Math.max(ryj, yj);

    }

     /**
     *  Deze methode tekent alle gevonden lege vakjes op het scherm
     * 
     * @param position
     * @param measures
     * @param sensor Laser of Sonar
     */
    public void drawEmpties(double position[], double measures[], Device sensor) {
        // correctie groot 20cm: sensorpunt robot staat 20 cm voor referentiepunt (=position) robot
        // hier wordt dus vastgesteld waar het sensorpunt zich exact bevindt
        double rx = Math.round(position[0] + 20.0 * Math.cos(Math.toRadians(position[2])));
        double ry = Math.round(position[1] + 20.0 * Math.sin(Math.toRadians(position[2])));
        int dir = (int) Math.round(position[2]);

        for (int i = 0; i < 360; i++) {
            int d = i - dir;
            while (d < 0) {
                d += 360;
            }
            while (d >= 360) {
                d -= 360;
            }
            // coordinaten sensorpunt + afstand meting geeft de positie van het nieuwe punt
            // dit is einde sensorbeam (niets gedetecteerd) of rand obstacles
            double fx = Math.round(rx + measures[d] * Math.cos(Math.toRadians(i)));
            double fy = Math.round(ry + measures[d] * Math.sin(Math.toRadians(i)));

            fillInEmpty(rx, ry, fx, fy);
        }

        //---tekenen---
        this.repaint();
        //-------------
    }
    
    /**
     *  Deze methode vult de occupancymap aan met de gevonden lege vakjes
     * 
     * @param rx x-position begin sensorbeam
     * @param ry y-position 
     * @param x  x-position eind sensorbeam
     * @param y  y-position
     */
    private void fillInEmpty(double rx, double ry, double x, double y) {
        // ceil = the smallest (closest to negative infinity) floating-point value
        //        that is not less than the argument and is equal to a mathematical integer.
        int rxi = (int) Math.ceil(rx / cellDim);
        int ryj = (int) Math.ceil(ry / cellDim);
        int xi = (int) Math.ceil(x / cellDim);
        int yj = (int) Math.ceil(y / cellDim);

        // niet buiten bereik occupancy grid treden
        if (xi < 0 || yj < 0 || xi >= width / cellDim || yj >= height / cellDim) {
            return;
        }

        int xmin = Math.min(rxi, xi);
        int xmax = Math.max(rxi, xi);
        int ymin = Math.min(ryj, yj);
        int ymax = Math.max(ryj, yj);

        if (rx == x) {
            for (int j = ymin; j <= ymax; j++) {
                if (grid[xmin][j] != OBSTACLE && grid[xmin][j] != OPAQUEOBSTACLE) {
                    grid[xmin][j] = EMPTY;
                }
            }
        } else {
            double m = (y - ry) / (x - rx);
            double q = y - m * x;
            for (int i = xmin; i <= xmax; i++) {
                int h = (int) Math.ceil((m * (i * cellDim) + q) / cellDim);
                if (h >= ymin && h <= ymax) {
                    if (grid[i][h] != OBSTACLE && grid[i][h] != OPAQUEOBSTACLE) {
                        grid[i][h] = EMPTY;
                    }
                }
            }
        }
    }
    
    /**
     * set indicator ten teken dat map compleet is explored
     * 
     * @param isExplored true if map is explored
     */
    public void setMapIsExplored(boolean isExplored){
            this.mapIsExplored = isExplored;
        }
     
    /**
     * Fill grid overlay with by robot explored path
     * @param x position
     * @param y position
     */
    public void setGridOverlay(int x, int y) {
        gridOverlay[x / cellDim][y / cellDim] = TOUCHED;
    }

    /**
     * 
     * @return Empty character
     */
    public static char getEMPTY() {
        return EMPTY;
    }

    /**
     * 
     * @return Obstacle character
     */
    public static char getOBSTACLE() {
        return OBSTACLE;
    }

    /**
     * 
     * @return Unknown character
     */
    public static char getUNKNOWN() {
        return UNKNOWN;
    }

    /**
     * 
     * @return Opaqueobstacle character
     */
    public static char getOPAQUEOBSTACLE() {
        return OPAQUEOBSTACLE;
    }

    /**
     * 
     * @return cellDim Dimension of one grid cell
     */
    public int getCellDim() {
        return cellDim;
    }

    /**
     * een refentie naar de occupancy map informatie
     * @return grid 
     */
    public char[][] getGrid() {
        return grid;
    }

    @Override
    public int getWidth() {
        return width;
    }

    @Override
    public int getHeight() {
        return height;
    }
    
}

